/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <mex.h>



#define zeros(X,Y) mxCreateDoubleMatrix(X,Y,mxREAL);
#define sendup(X) mxSetName(X,#X); mexPutArray(X,"base")
/*#define FIX(X) sign(X)*floor(fabs(X));*/ 


#define BASETYPE2 double
#define BASETYPE  unsigned char
#define MAGICK_NUMBER 6
#ifndef MAX
# define MAX(a,b) (((a) > (b)) ? (a) : (b))
#endif
#ifndef MIN
# define MIN(a,b) (((a) < (b)) ? (a) : (b))
#endif
#define PI 3.14159265358979


/*mex -v integrate_gyros.c */

void    ldumat(double *,double *,int,int);
void    ldvmat(double *,double *,int );
int     qrbdv(double *,double *,double *,int ,double *,int);
void    calcdev(int *,double *, double *, double *, double *, double  *, int, double);
void    calcdevh(double *,double *, double *, double *, double  *, int, int *);
int     svduv(double *,double *,double *,int,double *,int);
void    hpsrt(double *,int *, int);

void integrate_gyros(double *qua,double *gyros,int N)
{
    double tq[4],*nq,q[4],theta;
    int i;

    for(i=0;i<N;i++)
    {
    	theta=sqrt(gyros[3*i]*gyros[3*i]+gyros[3*i+1]*gyros[3*i+1]+gyros[3*i+2]*gyros[3*i+2]);
        if(fabs(theta)<0.00000000001)
        {
            q[0]=1;
            q[1]=0;
            q[2]=0;
            q[3]=0;
        }
        else
        {	
            q[0]=cos(theta/2);
            q[1]=sin(theta/2)*gyros[3*i]/theta;
            q[2]=sin(theta/2)*gyros[3*i+1]/theta;
            q[3]=sin(theta/2)*gyros[3*i+2]/theta;
        }

	nq=qua+i*4;
        tq[0]=q[0]*nq[0]-q[1]*nq[1]-q[2]*nq[2]-q[3]*nq[3];
        tq[1]=q[0]*nq[1]+q[1]*nq[0]+q[2]*nq[3]-q[3]*nq[2];
        tq[2]=q[0]*nq[2]-q[1]*nq[3]+q[2]*nq[0]+q[3]*nq[1];
        tq[3]=q[0]*nq[3]+q[1]*nq[2]-q[2]*nq[1]+q[3]*nq[0];
	/*
	    v3[0]=v1[0]*v2[0]-v1[1]*v2[1]-v1[2]*v2[2]-v1[3]*v2[3];
            v3[1]=v1[0]*v2[1]+v1[1]*v2[0]-v1[2]*v2[3]+v1[3]*v2[2];
            v3[2]=v1[0]*v2[2]+v1[1]*v2[3]+v1[2]*v2[0]-v1[3]*v2[1];
            v3[3]=v1[0]*v2[3]-v1[1]*v2[2]+v1[2]*v2[1]+v1[3]*v2[0];
    
	tq(1)=q(1)*nq(1)-q(2)*nq(2)-q(3)*nq(3)-q(4)*nq(4);
        tq(2)=q(1)*nq(2)+q(2)*nq(1)+q(3)*nq(4)-q(4)*nq(3);
        tq(3)=q(1)*nq(3)-q(2)*nq(4)+q(3)*nq(1)+q(4)*nq(2);
        tq(4)=q(1)*nq(4)+q(2)*nq(3)-q(3)*nq(2)+q(4)*nq(1);
	
	tq(1)=q(1)*nq(1)-q(2)*nq(2)-q(3)*nq(3)-q(4)*nq(4);
        tq(2)=q(1)*nq(2)+q(2)*nq(1)-q(3)*nq(4)+q(4)*nq(3);
        tq(3)=q(1)*nq(3)+q(2)*nq(4)+q(3)*nq(1)-q(4)*nq(2);
        tq(4)=q(1)*nq(4)-q(2)*nq(3)+q(3)*nq(2)+q(4)*nq(1);
        R=quater2rot(q)
        R1=quater2rot(nq)
        R2=R*R1
        quater2rot(tq)
        q=rot2quater(Ro8)
        nq=X(1:4)

         tq[0]=X[D*j]*w[0]-X[D*j+1]*w[1]-X[D*j+2]*w[2]-X[D*j+3]*w[3];
        tq[1]=X[D*j]*w[1]+X[D*j+1]*w[0]+X[D*j+2]*w[3]-X[D*j+3]*w[2];
        tq[2]=X[D*j]*w[2]+X[D*j+2]*w[0]-X[D*j+1]*w[3]+X[D*j+3]*w[1];
        tq[3]=X[D*j]*w[3]+X[D*j+3]*w[0]+X[D*j+1]*w[2]-X[D*j+2]*w[1];
        
	tq[0]=X[D*j]*w[0]  -X[D*j+1]*w[1]-X[D*j+2]*w[2]-X[D*j+3]*w[3];
        tq[1]=X[D*j+1]*w[0]+  X[D*j]*w[1]  -X[D*j+3]*w[2]+X[D*j+2]*w[3];
        tq[2]=X[D*j+2]*w[0]+X[D*j+3]*w[1]+  X[D*j]*w[2] -X[D*j+1]*w[3];
        tq[3]=X[D*j+3]*w[0]-X[D*j+2]*w[1]+X[D*j+1]*w[2]+X[D*j]*w[3];
    

	*/
	nq=qua+i*4+4;

        nq[0]=tq[0];
        nq[1]=tq[1];
        nq[2]=tq[2];
        nq[3]=tq[3];
   //printf("q[0]:%.3f\n",360*acos(fabs(nq[0]))/PI);
    }
}

void gproj3D_fun(
		double *e,
		double *J,
		double *q,
		double *X
		)
{
	double R[12],z,x,y,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,a;
	double dRdq0[9],dRdq1[9],dRdq2[9],dRdq3[9];

	
	
	norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
	q1t2=q[0]*q[0];
	q2t2=q[1]*q[1];
	q3t2=q[2]*q[2];
	q4t2=q[3]*q[3];
	q1q2=q[0]*q[1];
	q2q3=q[1]*q[2];
	q3q4=q[2]*q[3];
	q1q4=q[0]*q[3];
	q2q4=q[1]*q[3];
	q1q3=q[0]*q[2];
	/* building Euclidian transformation matrix */

/*	R[0]=(q1t2+q2t2-q3t2-q4t2);
	R[1]=2*(q2q3+q1q4);
	R[2]=2*(q2q4-q1q3);
	R[3]=2*(q2q3-q1q4);
	R[4]=(q1t2-q2t2+q3t2-q4t2);
	R[5]=2*(q3q4+q1q2);
*/	
	R[6]=2*(q2q4+q1q3);
	R[7]=2*(q3q4-q1q2);
	R[8]=(q1t2-q2t2-q3t2+q4t2);

	a=2*norm*norm;
   

	dRdq0[6]= q[2]*norm*2-a*q[0]*R[6];
	dRdq0[7]=-q[1]*norm*2-a*q[0]*R[7];
	dRdq0[8]= q[0]*norm*2-a*q[0]*R[8];
	

	dRdq1[6]= q[3]*norm*2-a*q[1]*R[6];
	dRdq1[7]=-q[0]*norm*2-a*q[1]*R[7];
	dRdq1[8]=-q[1]*norm*2-a*q[1]*R[8]; 
     

	dRdq2[6]= q[0]*norm*2-a*q[2]*R[6];
	dRdq2[7]= q[3]*norm*2-a*q[2]*R[7];
	dRdq2[8]=-q[2]*norm*2-a*q[2]*R[8];
     

	dRdq3[6]= q[1]*norm*2-a*q[3]*R[6];
	dRdq3[7]= q[2]*norm*2-a*q[3]*R[7];
	dRdq3[8]= q[3]*norm*2-a*q[3]*R[8];
	

	R[6]*=norm;
	R[7]*=norm;
	R[8]*=norm;
    /* projecting object 3D-coordinates to camera frame */
	x=R[6]*X[2];
	y=R[7]*X[2];
	z=R[8]*X[2];

    /*residuals */
	e[0]=X[0]-x;
	e[1]=X[1]-y;
	e[2]=X[2]-z;
   
        
    /* motion parameters for x */
	J[0]=-1.*dRdq0[6];
	J[1]=-1.*dRdq1[6];
	J[2]=-1.*dRdq2[6];
	J[3]=-1.*dRdq3[6];
          

    /* motion parameters for y */
	J[0]=-1.*dRdq0[7];
	J[1]=-1.*dRdq1[7];
	J[2]=-1.*dRdq2[7];
	J[3]=-1.*dRdq3[7];
                 
     /* motion parameters for z */
	J[0]=-1.*dRdq0[8];
	J[1]=-1.*dRdq1[8];
	J[2]=-1.*dRdq2[8];
	J[3]=-1.*dRdq3[8];
                 
 
}

/******************************************************/
void mexFunction(
int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[]
)
{  

  
  const mxArray     *mgyros,*mq0;
  mxArray  *mq;
 
  double *gyros, *q, *q0;

  
  int	i = 0;
  int   N;
  

  
  static int	expectRhs = 2;
  static int	expectLhs = 1;
  char		buffer[100];

  /* Check for proper number of arguments */

  if (nrhs != expectRhs) 
  {
    sprintf(buffer, "integrate_gyros requires %d input arguments. Got %d",
	    expectRhs, nrhs);
    mexErrMsgTxt(buffer);
  } else if (nlhs > expectLhs) 
  {
    sprintf(buffer, "integrate_gyrosrequires %d outputs arguments. Got %d",
	    expectLhs, nlhs);
    mexErrMsgTxt(buffer);
  }
  
  mgyros = prhs[i++];
  mq0 = prhs[i++];

  
  N =(int) mxGetNumberOfElements(mgyros)/3;

  mq=zeros(4,N+1);

  q=mxGetPr(mq); 
  q0=mxGetPr(mq0); 
  gyros=mxGetPr(mgyros); 
  q[0]=q0[0];
  q[1]=q0[1];
  q[2]=q0[2];
  q[3]=q0[3];
  integrate_gyros(q,gyros,N);

  i = 0;
  
  plhs[i++] = mq;

 
}
